ov

2024-06-17 11:10| 来源: 网络整理| 查看: 265

static void ov_msckf::StateHelper::augment_clone(std::shared_ptr state, Eigen::Matrix last_w)

Augment the state with a stochastic copy of the current IMU pose.

Parameters state Pointer to state last_w The estimated angular velocity at cloning time (used to estimate imu-cam time offset)

After propagation, normally we augment the state with an new clone that is at the new update timestep. This augmentation clones the IMU pose and adds it to our state's clone map. If we are doing time offset calibration we also make our cloning a function of the time offset. Time offset logic is based on Li and Mourikis [28].

We can write the current clone at the true imu base clock time as the follow:

\begin{align*} {}^{I_{t+t_d}}_G\bar{q} &= \begin{bmatrix}\frac{1}{2} {}^{I_{t+\hat{t}_d}}\boldsymbol\omega \tilde{t}_d \\ 1\end{bmatrix}\otimes{}^{I_{t+\hat{t}_d}}_G\bar{q} \\ {}^G\mathbf{p}_{I_{t+t_d}} &= {}^G\mathbf{p}_{I_{t+\hat{t}_d}} + {}^G\mathbf{v}_{I_{t+\hat{t}_d}}\tilde{t}_d \end{align*}

where we say that we have propagated our state up to the current estimated true imaging time for the current image, ${}^{I_{t+\hat{t}_d}}\boldsymbol\omega$ is the angular velocity at the end of propagation with biases removed. This is off by some smaller error, so to get to the true imaging time in the imu base clock, we can append some small timeoffset error. Thus the Jacobian in respect to our time offset during our cloning procedure is the following:

\begin{align*} \frac{\partial {}^{I_{t+t_d}}_G\tilde{\boldsymbol\theta}}{\partial \tilde{t}_d} &= {}^{I_{t+\hat{t}_d}}\boldsymbol\omega \\ \frac{\partial {}^G\tilde{\mathbf{p}}_{I_{t+t_d}}}{\partial \tilde{t}_d} &= {}^G\mathbf{v}_{I_{t+\hat{t}_d}} \end{align*}

【本文地址】

公司简介

联系我们